#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Int8.h>

#include <map>

#include "zbar.h"

typedef enum Destination {
  DST_A,
  DST_B,
  DST_C,
  DST_D,
} Destination;

class ImageConverter {
  ros::NodeHandle nh_;
  ros::Publisher task_goal_pub_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  std::map<std::string, Destination> string_to_destination = {
      {"A", Destination::DST_A},
      {"B", Destination::DST_B},
      {"C", Destination::DST_C},
      {"D", Destination::DST_D}};

 public:
  ImageConverter() : nh_("~"), it_(nh_) {
    image_sub_ = it_.subscribe("/camera/color/image_raw", 1,
                               &ImageConverter::imageCb, this);
    task_goal_pub_ = nh_.advertise<std_msgs::Int8>("/task_goal", 1);
  }

  ~ImageConverter() {}

  void imageCb(const sensor_msgs::ImageConstPtr& msg) {
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    ROS_INFO_ONCE("Got camera data!");

    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
    cv::Mat frame_grayscale;
    cv::cvtColor(cv_ptr->image, frame_grayscale, CV_BGR2GRAY);
    zbar::Image image(cv_ptr->image.cols, cv_ptr->image.rows, "Y800",
                      (uchar*)frame_grayscale.data,
                      cv_ptr->image.cols * cv_ptr->image.rows);
    scanner.scan(image);

    for (zbar::Image::SymbolIterator symbol = image.symbol_begin();
         symbol != image.symbol_end(); ++symbol) {
      std_msgs::Int8 msg;
      std::string qr_result = symbol->get_data();
      if (string_to_destination.find(qr_result) ==
          string_to_destination.end()) {
        ROS_ERROR("Unknown QRcode content: %s", qr_result.c_str());
        continue;
      }
      msg.data = string_to_destination[qr_result];
      task_goal_pub_.publish(msg);
      ROS_INFO("I see %s!", qr_result.c_str());
    }

    image.set_data(nullptr, 0);
  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "qr_code_reader");
  ImageConverter ic;
  ROS_INFO("QRcode reader node is up!");
  ros::spin();
  return 0;
}
